#!/usr/bin/env python
# -*- coding: utf-8 -*-
# @Date    : 2022-11-17 20:24:48
# @Author  : BrightSoul (653538096@qq.com)



import numpy as np
import time
import PX4MavCtrlV4 as PX4MavCtrl




VehilceNum = 5
MavList = []

host_ip = "127.0.0.1"

# Create MAV instance
mavL = [PX4MavCtrl.PX4MavCtrler(20100+i*2, host_ip) for i in range(VehilceNum)]
time.sleep(2)



offsetL = [
    np.array([0,0]),
    np.array([-2,0]),
    np.array([-4,0]),
    np.array([0,-2]),
    np.array([-2,-2]),
]


takeoffL = [
    np.array([0,0]),
    np.array([4,0]),
    np.array([4,-2]),
    np.array([2,2]),
    np.array([4,2]),
]

print("init!")
for mav in mavL:
    mav.InitMavLoop()
    mav.InitTrueDataLoop()
time.sleep(2)



print("start!")
for mav in mavL:
    mav.initOffboard()


print("offboard!")
# fly to 10m high above its takeoff position
for mav,takeoff in zip(mavL,takeoffL):
    mav.SendPosNED(*takeoff, -10, 0)
    mav.SendCopterSpeed(3)
input("please input a key:")


print("swarm!")
for mav in mavL:
    mav.SendPosNED(100, 0, -10, 0)
    mav.SendCopterSpeed(0.5)
time.sleep(10)


